#include "../include/kinova_driver_qt/kinovaController.hpp"
#include <QMessageBox>
#include <QtTest/QTest>
#include <ros/ros.h>
namespace kinova_driver_qt
{

kinovaController::kinovaController(QHostAddress core_ipAddress, int kinova_controller_port,
                                   QMutex *_mutex, QWaitCondition *_conditionHandler,
                                   kinova_status *_kinova_status):
    mutex(_mutex),
    conditionHandler(_conditionHandler),
    kinovaStatus(_kinova_status)
{
    statusHandler = new kinovaStatusHandler;
    kinovaControllerPort = kinova_controller_port;
    coreIpAddress = core_ipAddress;
    udpSocket = new QUdpSocket;
    udpSocket->connectToHost(core_ipAddress, kinova_controller_port);

    start();
}

kinovaController::kinovaController( int kinova_controller_port )
{
    kinovaControllerPort = kinova_controller_port;
    udpSocket = new QUdpSocket;
}

void kinovaController::init(QHostAddress core_ipAddress)
{
    coreIpAddress = core_ipAddress;
    udpSocket->connectToHost(core_ipAddress, kinovaControllerPort);
}

kinovaController::~kinovaController()
{
}

void kinovaController::sendCommand(QByteArray byteArray)
{
    conditionHandler->wait(mutex,10);
    udpSocket->writeDatagram(byteArray, coreIpAddress, kinovaControllerPort);
}

QByteArray kinovaController::waitForResponse()
{
    QByteArray datagram;
    try
    {
        ROS_INFO("A");
        datagram = QByteArray();
        ROS_INFO("B");

        /*if(!udpSocket->waitForReadyRead())
        {
            ROS_INFO("C");
            ROS_INFO("Waiting For response have been timeouted");
            return QByteArray();
        }*/
        while(!udpSocket->hasPendingDatagrams()){QTest::qSleep(1);}

        ROS_INFO("D");

        while (udpSocket->hasPendingDatagrams())
        {
            ROS_INFO("F");
            datagram.resize(udpSocket->pendingDatagramSize());
            QHostAddress sender;
            quint16 senderPort;
            udpSocket->readDatagram(datagram.data(), datagram.size(),
                                    &sender, &senderPort);
        }
    }
    catch(...)
    {
        ROS_ERROR("Exception occurred");
    }
    return datagram;
}

bool kinovaController::kinova_goto_homePos()
{
    QByteArray byteArray;
    QXmlStreamWriter xmlWriter(&byteArray);
    xmlWriter.writeStartDocument();
    xmlWriter.writeStartElement("jaco");
    xmlWriter.writeStartElement("gotopos");
    xmlWriter.writeAttribute("mode", "home");
    xmlWriter.writeEndElement();
    xmlWriter.writeEndElement();
    xmlWriter.writeCharacters("|");

    sendCommand(byteArray);

    QByteArray responseByteArray = waitForResponse();
    return parseResultXml(responseByteArray,"gotopos_home");
}

bool kinovaController::kinova_send_trajectories(QList<Cartesian> trajectories)
{
    QByteArray byteArray;
    QXmlStreamWriter xmlWriter(&byteArray);
    xmlWriter.writeStartDocument();
    xmlWriter.writeStartElement("jaco");
    xmlWriter.writeStartElement("gotoTrajectories");
    xmlWriter.writeAttribute("mode", "cartesian");
    for(int id = 0; id<trajectories.size(); id++)
    {
        xmlWriter.writeStartElement("trajectory");
        xmlWriter.writeAttribute("id",QString::number(id));

        xmlWriter.writeStartElement("position");
        xmlWriter.writeTextElement("x", QString::number(trajectories[id].position.x) );
        xmlWriter.writeTextElement("y", QString::number(trajectories[id].position.y) );
        xmlWriter.writeTextElement("z", QString::number(trajectories[id].position.z) );
        xmlWriter.writeEndElement();

        xmlWriter.writeStartElement("rotation");
        xmlWriter.writeTextElement("x", QString::number(trajectories[id].rotation.x) );
        xmlWriter.writeTextElement("y", QString::number(trajectories[id].rotation.y) );
        xmlWriter.writeTextElement("z", QString::number(trajectories[id].rotation.z) );
        xmlWriter.writeEndElement();

        xmlWriter.writeEndElement();
    }
    xmlWriter.writeEndElement();
    xmlWriter.writeEndElement();
    xmlWriter.writeCharacters("|");

    sendCommand(byteArray);

    QByteArray responseByteArray = waitForResponse();
    return parseResultXml(responseByteArray, "gotoTrajectories");
}

bool kinovaController::kinova_set_cpos(Cartesian cpos)
{
    ROS_INFO("Writing XML request");
    QByteArray byteArray;
    QXmlStreamWriter xmlWriter(&byteArray);
    xmlWriter.writeStartDocument();
    xmlWriter.writeStartElement("jaco");
    xmlWriter.writeStartElement("gotopos");
    xmlWriter.writeAttribute("mode", "cartesian");
    xmlWriter.writeStartElement("position");
    xmlWriter.writeTextElement("x", QString::number(cpos.position.x) );
    xmlWriter.writeTextElement("y", QString::number(cpos.position.y) );
    xmlWriter.writeTextElement("z", QString::number(cpos.position.z) );
    xmlWriter.writeEndElement();

    xmlWriter.writeStartElement("rotation");
    xmlWriter.writeTextElement("x", QString::number(cpos.rotation.x) );
    xmlWriter.writeTextElement("y", QString::number(cpos.rotation.y) );
    xmlWriter.writeTextElement("z", QString::number(cpos.rotation.z) );
    xmlWriter.writeEndElement();

    xmlWriter.writeEndElement();
    xmlWriter.writeEndElement();
    xmlWriter.writeCharacters("|");
    ROS_INFO("Sending XML request");

    sendCommand(byteArray);

    ROS_INFO("XML request sent, waiting for response");
    QByteArray responseByteArray = waitForResponse();
    ROS_INFO("Response received");
    if(responseByteArray.isNull() || responseByteArray.isEmpty())
        return false;
    qDebug()<<responseByteArray;
    bool result = parseResultXml(responseByteArray,"gotopos_cartesian");
    return result;
}

bool kinovaController::kinova_set_apos(Joints apos)
{
    QByteArray byteArray;
    QXmlStreamWriter xmlWriter(&byteArray);
    xmlWriter.writeStartDocument();
    xmlWriter.writeStartElement("jaco");
    xmlWriter.writeStartElement("gotopos");
    xmlWriter.writeAttribute("mode", "angular");
    xmlWriter.writeStartElement("joints");
    xmlWriter.writeTextElement("joint_1", QString::number(apos.joint1) );
    xmlWriter.writeTextElement("joint_2", QString::number(apos.joint2) );
    xmlWriter.writeTextElement("joint_3", QString::number(apos.joint3) );
    xmlWriter.writeTextElement("joint_4", QString::number(apos.joint4) );
    xmlWriter.writeTextElement("joint_5", QString::number(apos.joint5) );
    xmlWriter.writeTextElement("joint_6", QString::number(apos.joint6) );
    xmlWriter.writeEndElement();
    xmlWriter.writeEndElement();
    xmlWriter.writeEndElement();
    xmlWriter.writeCharacters("|");

    sendCommand(byteArray);

    QByteArray responseByteArray = waitForResponse();
    return parseResultXml(responseByteArray,"gotopos_angular");
}

bool kinovaController::kinova_set_fingers(Fingers fingers)
{
    ROS_INFO("Generating request XML");
    QByteArray byteArray;
    QXmlStreamWriter xmlWriter(&byteArray);
    xmlWriter.writeStartDocument();
    xmlWriter.writeStartElement("jaco");
    xmlWriter.writeStartElement("gotopos");
    xmlWriter.writeAttribute("mode", "fingers");
    xmlWriter.writeStartElement("fingers");
    xmlWriter.writeTextElement("finger_1", QString::number(fingers.finger1) );
    xmlWriter.writeTextElement("finger_2", QString::number(fingers.finger2) );
    xmlWriter.writeTextElement("finger_3", QString::number(fingers.finger3) );
    xmlWriter.writeEndElement();
    xmlWriter.writeEndElement();
    xmlWriter.writeCharacters("|");
    ROS_INFO("Sending XML command to server");

    sendCommand(byteArray);

    ROS_INFO("Waiting for response from server");
    QByteArray responseByteArray = waitForResponse();
    ROS_INFO("Parsing response");
    if(responseByteArray.isEmpty() || responseByteArray.isNull())
        return false;
    qDebug()<<responseByteArray;
    bool result = parseResultXml(responseByteArray, "gotopos_fingers");
    ROS_INFO("Response parsed");
    return result;
}

bool kinovaController::parseResultXml(QByteArray byteArray, QString opType)
{
    if(byteArray.isNull() || byteArray.isEmpty())
    {
        QMessageBox::warning(0,"Error","byteArray is empty");
        return false;
    }
    ROS_INFO("Parsing Result");
    QXmlStreamReader xml(byteArray);
    QXmlStreamReader::TokenType token = xml.readNext();
    while( !xml.atEnd() &&
           !xml.hasError() )
    {
        if( token == QXmlStreamReader::StartDocument )
        {
            token = xml.readNext();
        }
        if( token == QXmlStreamReader::StartElement )
        {
            if( xml.name() == "jaco" )
            {
            }
            if( xml.name() == "type" )
            {
                if( xml.readElementText() != opType )
                {
                    return false;
                }
            }
            if( xml.name() == "result" )
            {
                return ( (xml.readElementText() == "SUCCEEDED")?true:false );
            }
        }
        token = xml.readNext();
    }
    return false;
}

Status kinovaController::kinova_get_status()
{
    Status status;
    ROS_INFO("getStatus->Sending request to kinova_driver_cs");
    QByteArray byteArray;
    QXmlStreamWriter xmlWriter(&byteArray);
    xmlWriter.writeStartDocument();
    xmlWriter.writeStartElement("jaco");
    xmlWriter.writeEmptyElement("status");
    xmlWriter.writeEndElement();
    xmlWriter.writeCharacters("|");

    sendCommand(byteArray);

    ROS_INFO("getStatus->Waiting for status Response");

    QByteArray responseByteArray = waitForResponse();

    int retry = 5;

    while( ( ( responseByteArray.isEmpty() || responseByteArray.isNull() ) && retry > 0 ) )
    {
        ROS_INFO("getStatus->There was a problem in received response retrying %d",retry);
        sendCommand(byteArray);
        responseByteArray = waitForResponse();
        retry--;
    }

    ROS_INFO("getStatus->Status Received, Parsing...");
    if(retry > 0)
    {
        status = statusHandler->parseStatus(responseByteArray);
    }
    return status;
}
bool kinovaController::kinova_stop()
{
    QByteArray byteArray;
    QXmlStreamWriter xmlWriter(&byteArray);
    xmlWriter.writeStartDocument();
    xmlWriter.writeStartElement("jaco");
    xmlWriter.writeEmptyElement("stop");
    xmlWriter.writeEndElement();
    xmlWriter.writeCharacters("|");

    sendCommand(byteArray);

    return true;
}

QString kinovaController::kinova_ping()
{
    QByteArray byteArray;
    QXmlStreamWriter xmlWriter(&byteArray);
    xmlWriter.writeStartDocument();
    xmlWriter.writeStartElement("jaco");
    xmlWriter.writeEmptyElement("ping");
    xmlWriter.writeEndElement();
    xmlWriter.writeCharacters("|");

    sendCommand(byteArray);

    ROS_INFO("getStatus->Waiting for status Response");
    QByteArray responseByteArray = waitForResponse();
    int retry = 30;

    while( (responseByteArray.isEmpty() || responseByteArray.isNull() ) && retry > 0 )
    {
        ROS_INFO("Ping->Retrying %d",retry);
        sendCommand(byteArray);
        responseByteArray = waitForResponse();
        retry--;
        sleep(1);
    }

    if(retry)
        ROS_INFO("Ping->Kinova Pinged");
    else
    {
        ROS_ERROR("Ping-> Failed in 30 tries");
        responseByteArray.append("NULL");
    }
    return QString(responseByteArray);
}
}
